/*
clsquare - closed loop simulation system
Copyright (c) 2004, Neuroinformatics Group, Prof. Dr. Martin Riedmiller,
University of Osnabrueck

Author: Martin Riedmiller

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.
   * Neither the name of the <ORGANIZATION> nor the names of its
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE. 
*/

/*! \mainpage CLSquare code documentation
 *
 * Authors: Martin Riedmiller, Roland Hafner, Sascha Lange, Stephan Timmer \n
 * Neuroinformatics Group \n
 * University of Osnabrueck \n
 *
 * \section intro_sec Introduction
 * \f$CLS^2\f$ (pronounced: clsquare)  provides a standardized framework for testing  - not only - Reinforcement Learning
 *  agents (called \c controller in the following) in a growing number of different environments
 *  (called \c plant in the following; Plant ). Special care is taken on the easy integration of new plants and controllers. By this,
 *  we want to encourage people to contribute to the system by sharing their plants and controllers.
 *
 * \subsection features Features:
 *
 * \li provides a standarized control loop with built-in documentation facilities as used in typical Reinforcement Learning tasks
 * \li easy combination of different plants and controllers via definition in a configuration file
 * \li equal treatment of real and simulated plants
 * \li standardized, external graphical interface
 * \li lean interfaces for plants and controllers using standard data types only, allow a quick migration to and from 
 \f$CLS^2\f$ from and to other software environments
 * \li prototypical implementations of plant and controller make the implementation of new
 systems easy
 * \li distinction between plant state and observed state allows implementation of POMDPs.
 * 
 * \section install_sec Installation
 *
 * \subsection step1 Step1: Get a fresh copy of CLSquare
 *
 * In the moment we do not provide precompiled versions. So you have to get the newest sources from:
 * \li http://www.ni.uos.de
 *
 * \subsection step2 Step2: Unpack the source code
 *
 * \li tar -xzv clsquare.2.0.tgz
 *
 * \subsection step3 Step3: Compile the main tree
 *
 * \li cd CLSquare/src
 * \li make
 * 
 *  
 * 
 */

#include <signal.h>		//allows deinit when killed with crtl-c

#include <iostream>
#include <cstring>
#include <stdio.h>
#include <sys/time.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>

#include "valueparser.h"
#include "output.h"
#include "statistics.h"
#include "input.h"
#include "plant.h"
#include "graphic.h"
#include "defaultgraphic.h"
#include "controller.h"
#include "registry.h"
#include "global.h"

   // modules oof the CLSquare control loop
Plant       *plant;           // Realizes a reinforcement rearning environment
Controller  *controller;      // Realizes a reinforcement learning agent 
Output       output;          // outputs information about state transitions occuring on an episode
Graphic     *graphic;         // visualizes the environment and the agent
Statistics   statistics;      // stores statistics about the learning process 
Input        input;           // computes initial states for an episode
int          verbosity;       // determines the level of detail for log-outputs 
int pipe2cls; // pipe for communication
const char* pipename = "/tmp/pipe2cls";


   
/** specification of the task */
struct spec_type {
  long cycles_per_episode;     // maximal number of control cycles of an episode
  long num_episodes;           // maximal number of episodes
  long call_cmd_freq;          // frequency of calling testing scripts (tests the learned policy)
  char call_cmd[MAX_STR_LEN];  // linux command for calling testing scripts
  long sleep_every_cycle;      // sleep time after every control cycle (necessary for visualization)
  bool interactive_mode;
  } spec; 

/** status of the system */
struct sys_type {
  int     state_dim;      // dimension of state space
  int     observation_dim;      // dimension of observation space
  int     action_dim;           // dimension of action space
  int     reference_input_dim;  // dimension of reference input
  double  delta_t;              // time delay [s] in between two state transitions
  double  *state;          // the current state of the plant
  double  *observation;          // the observation of the current state
  double  *action;               // action chosen by the agent (controller)
  double  *reference_input;      // reference input for plant (if plant needs one)
  double  reward;                // reward given by the plant
  long    cycle_ctr;             // counter of control cycles in current episode
  long    episode_ctr;           // counter of episodes 
  long    total_num_of_cycles;   // number of control cycles since start of simulation loop
  double  episode_time;          // elapsed time [s] since start of current episode 
  double  total_time;            // elapsed time [s] since start of simulation loop
} sys;

double*  next_state_tmp; // temporal variable for next plant state


void set_defaults(){
  verbosity                  = 0;
  spec.cycles_per_episode    = 2;
  spec.num_episodes          = 1;
  spec.sleep_every_cycle     = 0;
  spec.call_cmd_freq         = 0;
  spec.interactive_mode = false;
  sprintf(spec.call_cmd,"%s","CallCmd");
}

#define PIPE_PAUSE 2
#define PIPE_EMPTY 0
#define PIPE_START 1
#define PIPE_STOP 3
#define PIPE_CONTINUE 4


void init_modules(const char *fname){
  char name[MAX_STR_LEN];
  ValueParser vp(fname, "Main");

  // /////////////////////////////// Plant ////////////////////////////////////
  sys.action_dim          = 0;
  sys.state_dim     = 0;
  sys.observation_dim     = 0;
  sys.reference_input_dim = 0;

  if (vp.get("plant",name,MAX_STR_LEN) < 0) {
    EOUT ("No configuration entry found for module 'plant'!");
    exit(1);
  }
  plant = 0;
  plant = PlantFactory::getThePlantFactory()->create(name);
  if (plant == 0) {
    EOUT ("No implementation found for plant: (" << string(name) << "]!");
    exit(1);
  }
  IOUTC(1, "Using plant:        (" << string(name) << ").");
  if(plant->init_main( sys.state_dim, sys.observation_dim, sys.action_dim, sys.reference_input_dim, sys.delta_t, fname) == false ) {    
    EOUT ("Init of plant: (" << string(name) << ") failed!");
    exit(1);
  }
  if(sys.state_dim <1) {    
    EOUT ("Init of plant: (" << string(name) << ") failed: state_dim: "<< sys.state_dim<<" <1!");
    exit(1);
  }
  if(sys.observation_dim <1) {    
      EOUT ("Init of plant: (" << string(name) << ") failed: observation_dim: " << sys.observation_dim<<" <1!");
      exit(1);
  }
  
  // //////////////////////////// Controller //////////////////////////////////

  if (vp.get("controller",name,MAX_STR_LEN) < 0) {
    EOUT ("No configuration entry found for module 'controller'!");
    exit(1);
  }
  controller = 0;
  controller = ControllerFactory::getTheControllerFactory()->create(name);
  if (controller == 0) {    
    EOUT ("No implementation found for Controller (" << string(name) << ")!");
    exit(1);
  }
  IOUTC(1, "Using controller:   (" << string(name) << ").");
  if(controller->init( sys.observation_dim, sys.action_dim, sys.delta_t, fname) == false) {    
    EOUT ("Init of controller: (" << string(name) << ") failed!");
    exit(1);
  }
  
  // ////////////////////////// Input / Ouptut ////////////////////////////////

  if(input.init(sys.state_dim, sys.action_dim, sys.reference_input_dim, sys.delta_t, fname) == false) {    
    EOUT("Init of module input failed!");
    exit(1);
  }
  if(output.init(sys.state_dim, sys.observation_dim, sys.action_dim, sys.reference_input_dim, sys.delta_t, fname) == false)  {    
    EOUT("Init of module output failed!");
    exit(1);
  }
  
  // //////////////////////////// Graphic /////////////////////////////////////

  
  if (vp.get("graphic",name,MAX_STR_LEN) <= 0) {
    graphic = new DefaultGraphic();
    IOUTC(1, "Using graphic:      (DefaultGraphic).");
  }
  else {
    graphic = 0;
    graphic = GraphicFactory::getTheGraphicFactory()->create(name);
    if (graphic == 0) {
      EOUT ("No implementation found for graphic: (" << string(name) << ")!");  
      exit(1);
    }
    IOUTC(1, "Using graphic:      (" << string(name) << ").");
  }
  
  if(graphic->init(sys.state_dim, sys.observation_dim, sys.action_dim, sys.reference_input_dim, sys.delta_t, fname) == false) {    
    EOUT ("Init of module graphic failed!");
    exit(1);
  }
  
  // ////////////////////////// Statistics ////////////////////////////////////
  if(statistics.init(sys.state_dim, sys.observation_dim, sys.action_dim, sys.delta_t, spec.cycles_per_episode, fname) == false)  {    
    EOUT("Init of module statistic failed!");
    exit(1);
  } 
  
  IOUTC(1, "All modules successfully initialized"
	<< "\n    state_dim           : " << sys.state_dim
	<< "\n    observation_dim     : " << sys.observation_dim
	<< "\n    action_dim          : " << sys.action_dim
	<< "\n    reference_input_dim : " << sys.reference_input_dim
  << "\n    delta_t             : " << sys.delta_t);
} 

void deinit_modules(){
  plant->deinit();
  delete plant;
  controller->deinit();
  delete controller;
  input.deinit();
  output.deinit();
  graphic->deinit();
  delete graphic;
  statistics.deinit();
  delete[] sys.state;
  delete[] next_state_tmp;
  delete[] sys.observation;
  if (sys.action!=0) delete[] sys.action;
  if (sys.reference_input!=0) delete[] sys.reference_input;

  if(spec.interactive_mode){
    close(pipe2cls);
    unlink(pipename); /*Falls  schon vorhanden ist*/
  }
}

void exithandler(int sig)
{
	cerr << "process killed by ctrl-c: trying to unload modules." << endl;
	deinit_modules(); 
	exit(0);
}

void init_system(){
  sys.state  = new double[sys.state_dim];
  next_state_tmp = new double[sys.state_dim];
  sys.observation  = new double[sys.observation_dim];
  if (sys.action_dim > 0) 
    sys.action = new double[sys.action_dim]; 
  else 
    sys.action = 0;
  if (sys.reference_input_dim > 0)
    sys.reference_input = new double[sys.reference_input_dim];
  else
    sys.reference_input = 0; 

  //  char* pipename = "./pipe2cls";

  if(spec.interactive_mode){
    //    cout<<"Interactive Mode activated"<<endl;
    unlink(pipename); /*Falls  schon vorhanden ist*/
    mkfifo(pipename, 0666);

    if((pipe2cls=open(pipename,O_NONBLOCK)) == - 1){
      fprintf(stderr, "Error: can't open the pipe2cls.....\n");
      exit(0);
    }
  }
} 



int check_pipe(){
  int result = PIPE_EMPTY;
  
  do{
    // setting up select for unblocked input
    fd_set rfds;
    struct timeval tv;
    char buf[1024];

    FD_ZERO(&rfds);
    FD_SET(pipe2cls, &rfds); // add filedescriptor pipe2cls to set
    //  tv.tv_sec = 10; tv.tv_usec = 0;  // set max. waiting time; 10s only for test
    tv.tv_sec = 0; tv.tv_usec = 1;  // no waiting, only if something is in pipe
    if (select(pipe2cls + 1, &rfds, NULL, NULL, &tv) >0) { // pipe is not empty
      read(pipe2cls, &buf, sizeof(buf));
      cout<<"\n**** read now from /tmp/pipe2cls : "<<buf<<endl;
      if(strncmp(buf,"start",5) == 0)
	result = PIPE_START;
      else if(strncmp(buf,"stop",4) == 0)
	result = PIPE_STOP;
      else if(strncmp(buf,"pause",5) == 0){
	result = PIPE_PAUSE;
      }
      else if(strncmp(buf,"plant_cmd",9) == 0){
	plant->notify_command_string(buf);
      }
      else if(strncmp(buf,"graphic_cmd",9) == 0){
	graphic->notify_command_string(buf);
      }
      else if(strncmp(buf,"control_cmd",9) == 0){
	controller->notify_command_string(buf);
      }
      if(result == PIPE_PAUSE)  // notify plant of pause, to probably stop plant
	plant->notify_command_string(const_cast<char*>("plant_cmd pause"));  /// \todo: should become const char*
      cout<<"->result: "<<result<<endl<<endl;
    }
  } while(result == PIPE_PAUSE);  // pause
  
    return result;
}


void do_loop(){
  bool do_continue = true;
  bool is_terminal_state = false; 
  
  sys.cycle_ctr     = 1;
  sys.episode_time = 0;

  if (sys.cycle_ctr > spec.cycles_per_episode)
    do_continue = false;

  while(do_continue) {   
    input.get_reference_input(sys.reference_input, sys.cycle_ctr);
      
      if(sys.cycle_ctr == 1){ // first cycle in sequence
	bool      accepted  = true;
	long int  patience  = 0;	

	// try to set an initial state. if plant or controller refuses, get another one. 
	do{ 
	  accepted = true; // reset!
	  input.get_initial_state(sys.state);
	  if (!plant->check_initial_state(sys.state)) {
	    IOUTC(1, "Plant refused initial state, try another one ..."  << patience );
	    accepted = false;
	  }
	  plant->get_observation(sys.state, sys.reference_input, sys.observation);
	  if (!controller->check_initial_state_and_observation(sys.state, sys.state_dim, sys.observation)) {
	    IOUTC(1, "Controller refused initial state or observation, try another one ..." << patience );
	    accepted = false;
	  }
	} while(accepted == false && patience++ <100000);
	
	if(accepted == false){ // cannot find a reasonable starting state; exit
	  EOUT("Cannot find a reasonable initial state. Exiting");
	  exit(1);
	}
	
	plant->notify_episode_starts();
	controller->notify_episode_starts(sys.observation);
	statistics.notify_episode_starts(sys.state, sys.observation, sys.episode_ctr);
      }
      else	
	plant->get_observation(sys.state, sys.reference_input, sys.observation);
    
      if (!controller->get_action(sys.observation, sys.action)) {
	do_continue = false;
	IOUTC(2, "controller ended this episode.");
      }   
    
      is_terminal_state = !plant->next_state(sys.state, sys.action, sys.reference_input, next_state_tmp, sys.reward);           
      do_continue &= !is_terminal_state;     
      if (is_terminal_state)        
	IOUTC(2, "plant ended this episode.");      
    
      controller->notify_reward(sys.reward);
            
      do_continue &= (graphic->notify(sys.state, sys.observation, sys.reference_input, sys.action, sys.reward,
				      sys.cycle_ctr, sys.episode_ctr,	sys.episode_time, sys.total_time, 
				      sys.total_num_of_cycles)==true);     
      do_continue &= (output.notify(sys.state, sys.observation, sys.reference_input, sys.action, sys.reward,
				    sys.cycle_ctr, sys.episode_ctr, sys.total_time, sys.episode_time, 
				    sys.total_num_of_cycles)==true);  
      do_continue &= (statistics.notify(sys.state, sys.observation, sys.action, sys.reward, sys.cycle_ctr, sys.episode_ctr, 
					sys.episode_time, sys.total_time, sys.total_num_of_cycles)==true);     
           
      for(int i=0;i<sys.state_dim;i++) sys.state[i] = next_state_tmp[i];
      if (sys.cycle_ctr >= spec.cycles_per_episode) do_continue = false;

      if(spec.interactive_mode){
	int result = check_pipe();
	if(result == PIPE_STOP){
	  do_continue = false;
	  exithandler(0);
	}
      }

      if(do_continue == true) {
	// prepare for next cycle
	sys.cycle_ctr ++;
	sys.total_time += sys.delta_t;
	sys.episode_time += sys.delta_t;
	if(spec.sleep_every_cycle>0) usleep(spec.sleep_every_cycle *1000);
      }
  }
  


  plant->get_observation(sys.state, sys.reference_input, sys.observation);
  //  plant->notify_episode_stops(sys.state,sys.action, sys.reward);  
  plant->notify_episode_stops(sys.state, sys.reward);  
#if 0
  cout<<"final observation:     ";
  for(int i=0;i<sys.observation_dim;i++) 
    cout<<sys.observation[i]<<" ";
  cout << endl;
#endif
  controller->notify_episode_stops(sys.observation, sys.reward, is_terminal_state);
  statistics.notify_episode_stops(sys.state, sys.observation, sys.reward, is_terminal_state, sys.episode_ctr);
} 


void do_episodes(){
  char call_cmd_tmp[MAX_STR_LEN + 20];

  sys.total_time          = 0;
  sys.total_num_of_cycles = 0;

  // do initialisation of state, observation, reference_input and action once before sequences start.
  for (int i=0; i<sys.state_dim;i++)      sys.state[i]     = 0;
  for (int i=0; i<sys.observation_dim;i++)      sys.observation[i]     = 0;
  for (int i=0; i<sys.reference_input_dim; i++) sys.reference_input[i] = 0;
  for (int i=0; i<sys.action_dim;i++)           sys.action[i]          = 0;

  if(spec.call_cmd_freq>0){ // if activated, always call call_cmd before cycling starts to do initialisation
    sprintf(call_cmd_tmp,"%s 0 0 0 ",spec.call_cmd); 
    system(call_cmd_tmp);
  }

  for(sys.episode_ctr = 1; sys.episode_ctr <= spec.num_episodes; sys.episode_ctr++) {    
    do_loop();
    sys.total_num_of_cycles += sys.cycle_ctr;
    if(spec.call_cmd_freq>0 && (sys.episode_ctr % spec.call_cmd_freq) == 0){
      sprintf(call_cmd_tmp,"%s %10ld %10ld %10.2g",spec.call_cmd, sys.episode_ctr, sys.total_num_of_cycles, sys.total_time);
      system(call_cmd_tmp);
    }
  }
}


void read_options(const char * fname) 
{
  ValueParser vp(fname,"Main");
  vp.get("verbosity",           verbosity);
  vp.get("num_episodes",        spec.num_episodes);
  vp.get("sleep_every_cycle",   spec.sleep_every_cycle);
  vp.get("cycles_per_episode",  spec.cycles_per_episode);
  vp.get("call_cmd_freq",       spec.call_cmd_freq);
  vp.get("call_cmd",            spec.call_cmd,MAX_STR_LEN);
  vp.get("interactive_mode",            spec.interactive_mode);
  vp.get("i_mode",            spec.interactive_mode);
} 

int main(int argc, char **argv) {

  // setup handlers
  signal(SIGINT, exithandler);
  signal(SIGHUP, exithandler);
  signal(SIGTERM, exithandler);
  signal(SIGKILL, exithandler);
  signal(SIGPIPE, SIG_IGN);

  if(argc <2){
    cerr<<"\nCall by "<<argv[0]<<" <params.cls> [random seed]"<<endl<<endl;
    cerr<<"To get a list of available modules use"
	<<" --list_plants, --list_controllers or --list_graphics." <<endl;
    exit(1);
  }

    if (strcmp(argv[1], "--list_plants") == 0) {
    char buf[10000];
    int ret = PlantFactory::getThePlantFactory()->listEntries(buf, 10000);
    IOUT("Available plants: \n" << buf );
    if (ret != 0) {
      cout << "ERROR: Running out of buffer. "
	   << "Could not list all available plants!" << endl;
    }
    return 0;
  }      
  else if (strcmp(argv[1], "--list_controllers") == 0) {
    char buf[10000];
    int ret = ControllerFactory::getTheControllerFactory()->listEntries(buf, 10000);
    IOUT("Available controllers:  \n" << buf );
    if (ret != 0) {
      cout << "ERROR: Running out of buffer. "
	   << "Could not list all available controllers!" << endl;
    }
    return 0;
  } 
  else if (strcmp(argv[1], "--list_graphics") == 0) {
    char buf[10000];
    int ret =  GraphicFactory::getTheGraphicFactory()->listEntries(buf, 10000);
    IOUT("Available graphics:  \n" << buf );
    if (ret != 0) {
      cout << "ERROR: Running out of buffer. "
	   << "Could not list all available graphic modules!" << endl;
    }
    return 0;
  } 
  
  if(argc == 3){
    long tmp_seed = atol(argv[2]);
    IOUT("setting seed to " << tmp_seed );
    srand48(tmp_seed);
    } 

  set_defaults();
  read_options(argv[1]);
  init_modules(argv[1]);
  init_system();
  do_episodes();
  deinit_modules(); 
  return 0;
}


